#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
#include "boost/thread/mutex.hpp"
#include "boost/thread/thread.hpp"
#include "ros/console.h"

class MrobotTeleop
{
public:
	MrobotTeleop();

private:
	void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
	void publish();

	ros::NodeHandle ph_, nh_;

	int linear_, angular_, deadman_axis_;
	double l_scale_, a_scale_;
	ros::Publisher vel_pub_;
	ros::Subscriber joy_sub_;

	geometry_msgs::Twist last_published_;
	boost::mutex publish_mutex_;
	bool deadman_pressed_;
	bool zero_twist_published_;
	ros::Timer timer_;

};

MrobotTeleop::MrobotTeleop():
	ph_("~"),
	linear_(1),
	angular_(0),
	deadman_axis_(4),
	l_scale_(0.175),
	a_scale_(0.9)
{
	ph_.param("axis_linear", linear_, linear_);
	ph_.param("axis_angular", angular_, angular_);
	ph_.param("axis_deadman", deadman_axis_, deadman_axis_);
	ph_.param("scale_angular", a_scale_, a_scale_);
	ph_.param("scale_linear", l_scale_, l_scale_);

	deadman_pressed_ = false;
	zero_twist_published_ = false;

	vel_pub_ = ph_.advertise<geometry_msgs::Twist>("cmd_vel", 10, true);
	joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &MrobotTeleop::joyCallback, this);

	timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&MrobotTeleop::publish, this));
}

void MrobotTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{ 
	geometry_msgs::Twist vel;
	vel.angular.z = a_scale_*joy->axes[angular_];
	vel.linear.x = l_scale_*joy->axes[linear_];
	last_published_ = vel;
	deadman_pressed_ = joy->buttons[deadman_axis_];
}

void MrobotTeleop::publish()
{
	boost::mutex::scoped_lock lock(publish_mutex_);

	if (deadman_pressed_)
	{
		vel_pub_.publish(last_published_);
		zero_twist_published_ = false;
	}
	else if(!deadman_pressed_ && !zero_twist_published_)
	{
		vel_pub_.publish(*new geometry_msgs::Twist());
		zero_twist_published_ = true;
	}
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "TeleopJoy");
	MrobotTeleop mrobot_teleop;

	ros::spin();
}